#include "task_assignment.h"

int main(int argc, char **argv)
{
    ros::init(argc, argv, "bucket_transport_node");
    ros::NodeHandle nh_;
    ros::Rate rate(30);

    AgvManager agvs_info;
    MissionManager missions_info;
    ChargerManager chargers_info;
    StorePlaceManger storeplaces_info;
    geometry_msgs::Pose pose;
    pose.position.x = 1.5;
    pose.position.y = 2.5;
    pose.position.z = 0;
    AgvConstInfo agv_info;
    agv_info.RobotId_ = 1;
    Agv agv("normal",AgvStatues::IDLE,pose,0.2,agv_info);
    geometry_msgs::Pose pose1;
    pose1.position.x = 1.2;
    pose1.position.y = 2.2;
    pose1.position.z = 0;
    AgvConstInfo agv_info1;
    agv_info1.RobotId_ = 2;
    Agv agv1("normal",AgvStatues::IDLE,pose1,0.2,agv_info1);
    agvs_info.AddAgv(agv);
    agvs_info.AddAgv(agv1);

    CargoInfo cargo;
    cargo.EmptyOrFilled = 0;
    geometry_msgs::Pose b_pose;
    b_pose.position.x = 5.0;
    b_pose.position.y = 6.0;
    b_pose.position.z = 0;
    geometry_msgs::Pose e_pose;
    e_pose.position.x = 5.1;
    e_pose.position.y = 6.1;
    e_pose.position.z = 0;
    MissionInfo missioninfo;
    missioninfo.MissionType_ = MissionType::TRANSPORT;
    missioninfo.mission_id_ = 1;
    missioninfo.MissionSection_ = "normal";
    missioninfo.Priority_ = 1;
    missioninfo.StartPose_ = b_pose;
    missioninfo.EndPose_ = e_pose;
    missioninfo.CargoInfo_ = cargo;
    MissionStatue missionstatue = MissionStatue::WAITING;
    Mission mission(missioninfo,missionstatue);
    missions_info.AddMission(mission);

    geometry_msgs::Pose c_pose;
    c_pose.position.x = 25.1;
    c_pose.position.y = 26.1;
    c_pose.position.z = 0;
    ChargerPoint charger(1,0.5,0.5,"charger1",c_pose,true);
    chargers_info.AddCharger(charger);

    geometry_msgs::Pose s_pose;
    s_pose.position.x = 11.1;
    s_pose.position.y = 12.1;
    s_pose.position.z = 0;
    StorePlace store(1,0.5,0.5,"store1",s_pose,true);
    storeplaces_info.AddStore(store);


    TaskAssignment A(agvs_info,missions_info,chargers_info,storeplaces_info);
    rate.sleep();

    ros::spin();
}
